#include "mat3x3.h"

#include "math.h"

using namespace math;

Mat3x3::Mat3x3()
{
	Identity();
}

Mat3x3::Mat3x3(float	Rad)
{
	BuildRotation(Rad);
}

Mat3x3::~Mat3x3()
{

}
void	Mat3x3::Identity()
{
	_11=1;	 _12=0;	 _13=0;	
	_21=0;	 _22=1;	 _23=0;	
	_31=0;	 _32=0;	 _33=1;	
}


void		Mat3x3::TransformVec2(const math::Vec2&	From, math::Vec2& To)
{
	To.x = (From.x * _11) + (From.y * _21) + _31;
	To.y = (From.x * _12) + (From.y * _22) + _32;
}
	
void		Mat3x3::BuildMatrix(float Rad, const math::Vec2& O)
{
	BuildRotation(Rad);
	_31=O.x;	_32=O.y;
}

void		Mat3x3::BuildTranslation(const math::Vec2& O)
{
	_11=1;		_12=0;		_13=0;	
	_21=0;		_22=1;		_23=0;	
	_31=O.x;	_32=O.y;	_33=1;	
}

void		Mat3x3::BuildRotation(float	Rad)
{
	float	cr = cosf(Rad);
	float	sr = sinf(Rad);

	_11=cr;		_12=sr;		_13=0;	
	_21=-sr;	_22=-cr;	_23=0;	
	_31=0;		_32=0;		_33=1;	
}

void		Mat3x3::BuildScale(const math::Vec2& S)
{
	_11=S.x; _12=0;		_13=0;	
	_21=0;	 _22=S.y;	 _23=0;	
	_31=0;	 _32=0;		_33=1;	
}
	
void		Mat3x3::Translate(const math::Vec2& O)
{
	// or do a full transform + multiply?
	_31+=O.x;	_32+=O.y;	
}

void		Mat3x3::Rotate(float	Rad)
{
	Mat3x3	Rot(Rad);
	Mat3x3	Temp(*this);
	Multiply(Temp, Rot, *this);
}

void		Mat3x3::Scale(const math::Vec2& Scale)
{
	_11*=Scale.x; 
	_22*=Scale.y;	
}

math::Vec2	Mat3x3::GetUp()
{
	return math::Vec2(_12, _22);
}

math::Vec2	Mat3x3::GetRight()
{
	return math::Vec2(_11, _21);
}

math::Vec2	Mat3x3::GetPos()
{
	return math::Vec2(_31, _31);

}
void	Mat3x3::Multiply(const Mat3x3& A, const Mat3x3& B, Mat3x3& Dest)
{
  for (int x=0; x<3; ++x)
  {
	for (int y=0; y<3; ++y)
	{
		float sum = 0;
		for (int z=0; z<3; ++z)		sum += A.m[x][z] * B.m[z][y];
		Dest.m[x][y] = sum;
	}
  }
}
